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[57] ABSTRACT 

The invention controls an under-actuated robot manip- 
ulator by first obtaining predetermined active joint ac- 
celerations of the active joints and the passive joint 
friction forces of the passive joints, then computing 
articulated body quantities for each of the joints from 
the current positions of the links, and finally computing 
from the articulated body quantities and from the active 
joint accelerations and the passive joint forces, active 
joint forces of the active joints. Ultimately, the inven- 
tion transmits servo commands corresponding to the 
active joint forces thus computed to respective ones of 
the joint servos. 

The computation of the active joint forces is accom- 
plished using a recursive dynamics algorithm. In this 
computation, an inward recursion is first carried out for 
each link beginning with the outermost link in order to 
compute the residual link force of each link from the 
active joint acceleration if the corresponding joint is 
active or from the known passive joint force if the cor- 
responding joint is passive. Then, an outward recursion 
is carried out for each link beginning with the innermost 
link in which the active joint force is computed from the 
residual force if the corresponding joint is active or the 
passive joint acceleration is computed from the residual 
link force if the corresponding joint is passive. 
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CONTROLLING UNDER-ACTUATED ROBOT 

ARMS USING A HIGH SPEED DYNAMICS 

ORIGIN OF THE INVENTION 5 

The invention-described herein was made in the per- 
formance of work under a NASA contract, and is sub- 
ject to the provisions of Public Law 96-517 (35 USC 
202) in which the contractor has elected not to retain 
title. 

BACKGROUND OF THE INVENTION 

1. Technical Field 

The invention relates to under-actuated robot manip- 15 
ulators and more particularly to a method and apparatus 
for controlling robot arms having plural joints, some of 
the joints being passive in that they have no joint servos, 
using a high speed recursive dynamics algorithm to 
solve for the active joint torque commands required to 2Q 
achieve a user-specified velocity profile of the end- 
effector and, if desired, of the joints themselves. 

2. Background Art 

1. Introduction 

An extensive amount of research on the kinematics, 
dynamics and control of robots has been carried out for 
regular (or fully-actuated) manipulators. Each degree of 
freedom of regular manipulators is active, i.e., it has 
associated with it an actuator which can be used to 30 
control the manipulator. However, many important 
applications include manipulators with passive degrees 
of freedom, i.e., degrees of freedom lacking control 
actuators. A passive degree of freedom can arise from 
either an absence of an actuator, failure of the actuator, 35 
or due to a mode of operation which precludes the use 
of the actuator. We refer to manipulators with passive 
degrees of freedom as under-actuated manipulators. 
Thus, for under-actuated manipulators, the number of 
available control actuators is less than the number of 40 
degrees of freedom. Examples of under-actuated manip- 
ulators include 

free-flying space robots with inactive reaction jets. 

hyper-redundant robots for whom not all hinges are 
actuated. 45 

manipulators with flexible joints and hinges. 

manipulators loosely grasping an articulated object. 

manipulators with actuator failures, and the design of 
fault-tolerant control algorithms. 

Each of these important areas has received varying 50 
degrees of attention from researchers, resulting in the 
development of useful, though largely application spe- 
cific techniques for the analysis and control of these 
systems. The usefulness and applicability of these tech- 
niques to other types of under-actuated manipulators is 55 
difficult to see. For instance, the large amount of re- 
search on free-flying space-robots has resulted in analy- 
sis techniques that make extensive use of the conserva- 
tion of momentum property. These techniques however 
cannot be applied to under-actuated systems where 60 
such momentum constraints do not hold. 

The analysis of under-actuated manipulators is signifi- 
cantly more complex when compared with regular 
manipulators. There is inertial coupling between the 
motion of the active and the passive hinges, so that 65 
mappings such as the Jacobian function depend not only 
on the kinematical properties, but also on the inertia 
properties of the links. The presence of passive degrees 


2 

of freedom also typically results in a lack of full control- 
lability of the system. 

In the remainder of this specification, the description 
herein refers to various individual publications listed 
below by number by reciting, for example, “reference 
[1]”, or “reference [2]”, or simply “[1” or “[2]”, and so 
forth. 
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Some early work on the control of such manipulators 
can be found in reference [1-3]. 

We make extensive use of techniques from the spatial 
operator algebra [4]. In Section 3 below, we review the 
spatial operator approach and develop the equations of 
motion for a regular manipulator. A model for under- 
actuated manipulators and their equations of motion is 
described in Section 4 below. Spatial operator identities 
are then used to develop closed form expressions for the 
generalized accelerations for the system. These expres- 
sions form the basis for a recursive 0(N) dynamics 
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algorithm described in Section 5 below. Expressions for 
the disturbance and generalized Jacobians that relate 
the motion of the active hinges to the motion of the 
passive hinges and to the motion of the end-effector 
respectively are developed in Section 6 below, and a 
system for controlling an under-actuated robot with 
redundant (i.e., 7 or more) joints using the disturbance 
Jacobian is described. 


4 

-continued 


manipulator 

Ap A a the passive and active manipulator subsystems of an 
under-actuated manipulator 

np n a the number of passive and active hinges respectively 
Np N a the number of passive and active degrees of freedom 
respectively 

Ip I a the set of indices of the passive and active hinges respectively 


2. Nomenclature 

Coordinate free spatial notation is used throughout 
this specification (see [4,5] for additional details). The 
notation f denotes the cross-product matrix associated 
with the 3-dimensional vector 1, while x* denotes the 
transpose of a matrix x. In the stacked notation used in 
this specification, indices are used to identify quantities 
pertinent to a specific link. Thus for instance, V denotes 
the vector of the spatial velocities of all of the links in a 
serial manipulator, and V(k) denotes the spatial velocity 
vector for the k th link. Some key quantities used in this 
specification are defined below. 


n number of links in the manipulator 

Ok body frame for k* h link 

0&+ outboard frame on the ( A + 1)' A link 

r{k) number of degrees of freedom for k* h joint 

N = r(k), the overall degrees of freedom for the manipulator 

$(k) € 52 the vector of generalized coordinates for the A™ hinge 

P(k) e K*), the vector of generalized velocities for the k* h hinge 

/(A,/) 3 , the vector from the A** to the/ A body frame 

4>ikJ) ^ ^ ^e3J 6x6 , the spacial transformation 

operator from the/ A hinge to the k th hinge 

H\k) eSJ£ 6xr M, the joint map matrix for the A^ hinge 
m(Jc) mass of the k* h link 

p{k) e 3J 3 , the vector from Ok to the center of 
mass of the A** link 

J(k) € §fj 3x3 , the inertia matrix for the k* h link about Ok 

(m m(mk) \ 

W ^-m(fc)p(*) m(k)h J X ' 

spatial inertia of the kP 1 link referred to Ok 


eSJJ 6 , the spatial velocity of the k? h link referred 

to Oh with o>(A) and v(A) denoting the angular and linear 
velocity components respectively 

the Coriolis acceleration for the link referred to Ok 

m = "I ,a‘. the centrifugal force for 

\m(k)o)(k)o)(k)p(k) J 

the k* h link referred to Ok 

a(k)e&*, the spatial acceleration of the k* h link referred to Ok 



1U 3. Dynamics of Regular Manipulators 

We consider a serial manipulator with n rigid body 
links. As shown in FIG. 1, the links are numbered in 
increasing order from tip to base. 

15 The outer most link is denoted link 1 and the inner 
most link is denoted link n. 

Each link has two frames denoted 0* and 0*+ at- 
tached to it. Frame O&is on the inboard side is the body 
2Q frame for the k th link. The k th hinge connects the 
k+1 ) th and k th links and its motion is defined as the 
motion of frame Ok with respect to frame 0&+i+. Free 
space motion of the manipulator is handled by using 6 
degree of freedom hinges between the base link and an 
25 inertial frame. The k th hinge is assumed to have r(k) 
degrees of freedom where l^r(k)^6, and its vector of 
generalized coordinates is denoted 0(k). For simplicity, 
and without any loss in generality, we assume that the 
number of generalized velocities for the hinge is also 
30 r(k), i.e., there are no nonholonomic constraints on the 
hinge. The vector of generalized speeds for the k^ hinge 
is denoted /3(k The choice of the hinge angle 
rates /§(k) for the generalized speeds /3(k) is often an 
35 obvious and convenient choice. However, when the 
number of hinge degrees of freedom is larger than 1, 
alternative choices are often preferred since they lead to 
simplifying decoupling of the kinematic and dynamic 
parts of the equations of motion. An example is the use 
40 of the relative angular velocity (rather than the Euler 
angle rates( for the generalized speeds vector in the case 
of a free-flying system. The overall number of degrees 
of freedom for the manipulator is given by 
45 N=2/ f= i«r(k). 

Coordinate free spatial notation is used throughout 
this specification (see [4,5]). The spatial velocity V(k) of 
the k th body frame Ok is defined as 

50 ✓ v 


55 with co(k) and v(k) denoting the angular and linear ve- 
locities of Oh The relative spatial velocity across the 
k th hinge is given by H*(k)/3(k) where H*(k)e 3£ 6x K*) is 
the joint map matrix for the hinge. The spatial force of 
interaction f(k) across the k th hinge is denoted 


Ak) = 



6 , the spatial force of interaction between 


the (A + \) th and the k* h link referred to Oh with N{k) and 
F(k) denoting the moment and force components respectively 


T\k)e3t the generalized force for the k :h hinge 
M € *^ x ^ the mass matrix for the manipulator 
C e vector of Coriolis and centrifugal forces for the 



with N(k) and F(k) denoting the moment and force 
components respectively. The spatial inertia M(k) of the 
k th link referred to Ok is defined as 
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M(k) 


_ ( J(k) m(k)Rk) } 




where 

M=H<j>M4>*H*€S iVxiV 
5 c£H<$>[M<$>*a+b]€$t N 

where m(k) is the mass, p(k)e£ 3 is the vector from O* H=diag{H(k)}, M=diag{M(k)}. 

to the center of mass, and J(k)e £ 3 x 3 is the inertia of the 
k th link about O&. 

With V(k) denoting the spatial velocity, a(k) the 10 
spatial acceleration, f(k) the spatial force and T(k) the 
hinge force at Ok for the k th link, the following Newton- 
Euler recursive equations [6,4] describe the equations of 
motion for the serial manipulator: 

15 


€ <J> : 


^ V{n + 1) = 0. a{n + 1) « 0 

for k ass «... 1 

m = 4>*(/c + \,k)V(k + 1) + H\k)m 
a(k) - + l y k)a(k + 1) + H\k)kk) 4- a(k) 

y end loop 

AO) = o 

for k — 1 ... n 

Ak) = <K* + 1 ,Wk - 1) + M(&)a(*) + 6(*) 

m = jw*) 

end loop 


(3.1) 


0 0 

4>(24) 0 

0 <K3,2) 


- i) o 


20 


25 




r'/ o 

4>(2,1) 7 




ON 

0 


with 


30 


(3.3a) 

(3.3b) 

(3.4a) 


(3.4b) 


where a(k) and b(k) denote the velocity dependent Co- 
riolis and centrifugal acceleration term and a gyro- 
scopic force term respectively. 4>(k,k — 1) denotes the 
transformation operator from Oa:~i to Ok and is given 35 
by 


<Hv) = - 1) . . . <#>(/ + 1 J) for I > ; 

The spatial transformation operator 4>(k,j) is defined as 


(:?*) 


e£ 6 x6 



We use spatial operators [4] to obtain compact ex- 
pressions for the equations of motion and other key 
dynamical quantities. As we see later, they are also very 45 
useful analysis tools. The vector 0=[0*(1), . . . 

denotes the vector of generalized coordi- 
nates for the manipulator. Similarly, we define the vec- 
tors of generalized speeds (3e$t N and generalized 
(hinge) forces Te£ ^for the manipulator. The vector of 50 
spatial velocities V is defined as V=[V*(1) . . . 
V*(n)]*e£6«- The vector of spatial accelerations is 
denoted aeS 6w , that of the Coriolis accelerations by 
aeS 6 ", the link centrifugal forces by beS 6rt , and the 
link interaction spatial forces by fe£ 6n . Note that the 55 
components of the vectors a and b are nonlinear func- 
tions of the velocities and expressions for them are 
given in Section 2. The equations of motion for the 
serial manipulator can be written as follows (see [4] for 
details): 60 


V=4>*H*£ 

(3.2a) 

a=4>*[i7*/34-ff] 

(3.2b) 

f—$[Ma+b] 

(3.2c) 

T—Hf~Mp+C 

(3.2d) 


with l(k,j)e£ 3 , denoting the vector from the k th to the 
j^body frame. The notation 1 denotes the cross-product 
matrix associated with the 3-dimensional vector 

M is the mass matrix of the manipulator and the vec- 
tor C contains the velocity dependent Coriolis and cen- 
trifugal hinge forces. External forces on any link in the 
manipulator are handled by adding their sum effect to 
the component of the b vector for that link. 

Using 0 as the index to represent the end-effector 
frame, the spatial velocity V(0) of the end-effector is 
given by 

V(0)= 4>*(1,0)V(1)=BV — B<f>*H*j8 
where the operator B is given by 

b==[4>*(i»o),o, . , . o]*€& 6 ” x6 

Thus the operator expression for the end-effector Jaco- 
bian is given by [4], 

J=B*4>*H* (3.5) 

The model described above for the manipulator with 
rigid links is general enough to carry over in essentially 
the same manner to manipulators (such as free-flying 
space robots, flexible link manipulators etc.) which have 
degrees of freedom which are not necessarily associated 
with physical hinges. This is possible mainly due to the 
use of the operators approach in which all types of 
manipulator degrees of freedom share essentially similar 
mathematical representations. Thus for flexible link 
manipulators, the detailed structure of 4 >( . . . ) and H(.) 
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needs to be appropriately modified [7]. The equations of 
motion for flexible link manipulators have the same 
operator form as in Eq. (3.2) and Eq. (3.3) above. For a 
free-flying space manipulator, the degrees of freedom 
associated with the base body can be modeled as a 6 
degree of freedom hinge between the base body and the 
inertial frame. Thus the use of the terminology hinge is 
not necessarily restricted to physical hinges alone. 

SUMMARY OF THE INVENTION 

The invention controls an under-actuated robot ma- 
nipulator by first obtaining predetermined active joint 
accelerations of the active joints and the passive joint 
friction forces of the passive joints, then computing 
articulated body quantities for each of the joints from 
the current positions of the links, and finally computing 
from the articulated body quantities and from the active 
joint accelerations and the passive joint forces, active 
joint forces of the active joints. Ultimately, the inven- 
tion transmits servo commands corresponding to the 
active joint forces thus computed to respective ones of 
the joint servos. 

The computation of the active joint forces is accom- 
plished using a recursive dynamics algorithm. In this 
computation, an inward recursion is first carried out for 
each link beginning with the outermost link. This in- 
ward recursion consists of computing a residual link 
force from a residual link force of a previous link, from 
a corresponding one of the active joint accelerations if 
the corresponding joint is active and from a correspond- 
ing one of the passive joint forces if the corresponding 
joint is passive. Then, an outward recursion is carried 
out for each link beginning with the innermost link. The 
outward recursion consists of computing a link acceler- 
ation from a link acceleration of a previous link, com- 
puting from the link acceleration an active joint force if 
the corresponding joint is an active joint, computing a 
passive joint acceleration if the corresponding joint is a 
passive joint, and revising the link acceleration based 
upon the joint acceleration of the corresponding joint. 

The articulated body quantities include an effective 
link inertia, a joint inertia, a joint to link force operator 
and a null force operator. They are computed in an 
inward recursion carried out for each link beginning at 
the outermost link. This latter inward recursion consists 
of computing the effective link inertia from the effective 
link inertia of a previous link, then, if the corresponding 
joint is passive, computing the joint inertia from the 
effective link inertia, the joint to link force operator 
from the effective link inertia and from the joint inertia, 
and the null force operator from the joint to link force 
operator, or, if the corresponding joint is active, setting 
the null force operator equal to an identity operator: 
and, finally, revising the effective link inertia by trans- 
forming it by the null force operator. 

The predetermined active joint accelerations are de- 
rived from the user’s specified motion of a particular 
link, such as the end effector or tip, for example, in a 
preferred embodiment of the invention. This is accom- 
plished by computing a generalized Jacobian operator 
relating the desired acceleration of the particular one 
link specified by the user to the active joint accelera- 
tions of the active joints, and then multiplying the de- 
sired acceleration of the selected one link by the gener- 
alized Jacobian operator to produce the active joint 
accelerations of the active joints. The generalized Jaco- 
bian operator is obtained using a modified version of the 
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recursive dynamic algorithm referred to above with 
reference to the computation of the active joint forces. 

For redundant under-actuated robot manipulators, 
the invention enables the user to employ well-known 
redundancy resolution techniques by imposing prede- 
termined motion constraints. For this purpose, the un- 
known passive joint accelerations are immediately com- 
puted by first computing a disturbance Jacobian opera- 
tor relating the unknown passive joint accelerations to 
the predetermined active joint accelerations, and then 
multiplying the active joint accelerations by the distur- 
bance Jacobian operator to produce the passive joint 
accelerations. The user than knows all joint accelera- 
tions, and may then use them to compute the motion 
parameters related to the predetermined motion con- 
straints, and from these, perform the well-known redun- 
dancy resolution techniques to control the robot manip- 
ulator. The disturbance Jacobian is formed from inter- 
mediate quantities produced during the modified recur- 
sive dynamics algorithm referred to above with refer- 
ence to computing the generalized Jacobian. 

BRIEF DESCRIPTION OF THE DRAWINGS 

FIG. 1 is a diagram illustrating a portion of an under- 
actuated robot manipulator and illustrating the coordi- 
nate system employed in the preferred embodiment of 
the invention. 

FIG. 2 is a diagram illustrating the decomposition of 
the under-actuated robot of FIG. 1 into active and pas- 
sive subsystems, in accordance with the invention. 

FIG. 3 is a block diagram illustrating the computation 
of the articulated body quantities of a system decom- 
posed in accordance with FIG. 2. 

FIG. 4 is a block diagram illustrating a general recur- 
sive dynamic algorithm for computing the active joint 
forces from the user-specified active joint accelerations. 

FIG. 5 a and FIG. 5 b comprise a block diagram illus- 
trating in detail the preferred embodiment of the algo- 
rithm of FIG. 4. 

FIG. 6 is a block diagram illustrating how the recur- 
sive dynamics algorithm of FIG. 4 is repeated over 
successive time steps to perform motion planning, for 
example. 

FIG. 7 is a block diagram illustrating a modified 
version of the recursive dynamics algorithm of FIG. 4 
used to compute the generalized Jacobian operator and 
the disturbance Jacobian operator. 

FIG. 8a and FIG. 8 b are block diagrams illustrating, 
respectively, (a) how the generalized Jacobian operator 
is used to computed unknown passive joint accelera- 
tions and (b) how the disturbance Jacobian operator is 
used in redundancy resolution techniques to control a 
redundant under-actuated robot manipulator. 

FIG. 9 is a block diagram illustrating a robot control 
system embodying the present invention. 

DETAILED DESCRIPTION OF THE 
INVENTION 

4. Modeling of Under- Actuated Manipulators 

We use the term active degree of freedom for manip- 
ulator degree of freedom which has associated with it a 
control actuator. A passive degree of freedom on the 
other hand is a manipulator degree of freedom with no 
control actuator. Due to the presence of friction, stiff- 
ness etc., the generalized force associated with a passive 
degree of freedom need not necessarily be zero. For a 
free-flying space manipulator with no reaction jets, all 
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the manipulator hinge degrees of freedom are active 
degrees of freedom. However, the positional and orien- 
tation degrees of freedom for the base-body are passive 
degrees of freedom. In the case of manipulators with 
link or joint flexibility, the degrees of freedom associ- 5 
ated with the flexible degrees of freedom are passive, 
while the hinge degrees of freedom are all active. 

We refer to hinges with all active (passive) degrees of 
freedom as active (passive) hinges. In general, the com- 
ponent degrees of freedom of multiple degree of free- ^ 
dom hinges may be a mix of active and passive degrees 
of freedom. However, we make the notationally conve- 
nient assumption that all the hinges in the manipulator 
are either active or passive hinges. This assumption 
imposes no loss in generality since any multiple degree 15 
of freedom hinge can be decomposed equivalently into 
a concatenation of active and passive hinges. Thus, for 
the purposes of discussion, we assume that a manipula- 
tor with hinges containing a mix of passive and active 2Q 
degrees of freedom has been replaced by an equivalent 
manipulator model consisting only of hinges which are 
either active or passive hinges. 

The number of passive hinges in the manipulator 
system is denoted np, and Ip denotes the set of indices of 25 
the passive hinges I a denotes the corresponding set of 
indices of the active hinges and n^ =(n— np) the number 
of active hinges in the manipulator system. The total 
number of passive degrees of freedom Np, is given by 
N P 30 

while the total number of active degrees of freedom N a , 
is given by N a 


( M aa Map 

(Pa) 


(c > 


(t > 

l a 

• 






w 


Cp 

\ P > 


T P 

\ V 


where with i,je{p,a} we have 

*H*j, and c£H$[b+M<S>*a] (4.3) 

Note that in Eq. (4.3), the s.ubmatrices M ca and Mp P are 
the mass matrices for the A a and A p arms respectively. 
The mathematically partitioned form in Eq. (4.2) by no 
means implies that the above formulation is restricted to 
manipulators in which the active and passive hinges are 
present in separate clusters. 

In order to control control the manipulator, it is im- 
portant to compute the actuator forces required to ob- 
tain a desired motion of the active hinges, and also the 
effect of this motion on the passive hinges. That is, it is 
necessary to compute the active hinge forces T a re- 
quired to obtain a desired active hinge acceleration /§ a , 
as well as the acceleration fi p induced at the passive 
hinges. The passive hinge forces vector T p is typically 
obtained from (frictional, stiffness etc.) models for the 
passive hinges. A simple rearrangement of eq. (4.2) puts 
it in the form 



where 


(= 2*^**)). 

Note that N c +Np=N. 

We use the sets I a and Ip to decompose the manipula- 
tor into a pair of component manipulator subsystems: 
the active arm A a , and the passive arm Ap, A a is the N a 
degree of freedom manipulator resulting from freezing 
all the passive hinges (i.e. all those whose index is in Ip), 
while Apis the Np degree of freedom manipulator result- 
ing from freezing all the active hinges (i.e. all those 
whose index is in the set I a ). This decomposition is 
illustrated in FIG. 2. 

Let p a ^ Na y Ta e?$t Na and H* a €£ 6/I+Ara denote the 
vector of generalized speeds, the vector of hinge forces 
and the joint map matrix for arm Aa. Similarly, let 
fi p e$l N p, Tpe$t N p and H *p C 3 ? 6 nXi V> denote the corre- 
sponding quantities for arm Ap. Note that the two vec- 
tors Pat %t Na and PpeJt N p , represent a decomposition of 
the vector of generalized speeds fi for the whole manip- 
ulator in a manner consistent with the sets l a and Ip 
respectively. Similarly T a and Tpare decompositions of 
T, and H* fl and H*p are decompositions of H*. The 
columns of H* which correspond to the active hinges 
appear as columns of H* a , while those that correspond 
to the passive hinges appear as columns of H*p. Thus it 
follows that 

(4.1) 

4.1 Equations of Motion 

We use the above decomposition to rewrite the equa- 
tions of motion in Eq. (3.2) in the following partitioned 
form: 


Saa = M m - M ap M~ l M ap ' < 4 - 5 > 

S ap = 

S - 

*PP ~ m pp 

In Eq. (4.4), the quantities to be computed appear on the 
right. The direct use of Eq. (4.4) to obtain P p and T a 
requires the computation of M, the inversion of Mppand 
the formation of various matrix/matrix and matrix/ vec- 
tor products. The computational cost of this dynamics 
algorithm is cubic in Np and quadratic in N fl . Another 
approach of similar computational complexity consists 
of adding additional constraints to the equations of 
motion of the system. 

The presence of the constraints requires additional 
steps such as constraint stabilization during the integra- 
tion of the equations of motion. Later, in Section 5 we 
describe a new 0(N) dynamics algorithm that over- 
comes these limitations. This new algorithm, does not 
require the computation of M, and its complexity is 
linear in both N a and Np. 

4.2 Under- Actuated Manipulators with Integrals, of 
Motion 

We now take a brief look at a special and important 
class of under-actuated manipulators which possess 
certain integrals of motion. The nonlinear velocity de- 
pendent term Cp has the form 

Cp^MpaZa+Mpppp-Wi&'Mft 

Thus we can rewrite the lower half of Eq. (4.2) (or 
equivalently Eq. (4.4)) in the form 
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d[Mpafi a + Mppfipl 1 „ _ ( 4 - 6 > 

— y V e/J [/3*M^] « T p 

However, if the under-actuated manipulator is such that 5 
the passive hinge generalized coordinates, 6 P , are ignor- 
able coordinates, i.e. 

Vfy[/3*M/3]=0 10 

then the left-hand side of Eq. (4.6) is an exact differen- 
tial and we can rewrite Eq. (4.6) in the form 

15 

r f < 4J > 

Mpofta + — [A/pQ, Mpp\l 3 — Tpdt -H constant 

J to 

Eq. (4.7) is equivalent to a time-varying, non-linear 2Q 
constraint on the generalized velocities of the system. If 
in addition. T p =0, the left-hand side of Eq. (4.7) is 
constant, and is indeed an integral of motion for the 
manipulator. 

Free-flying space manipulators are an important ex- 25 
ample of under-actuated manipulators with integrals of 
motion. For these manipulators, the reaction jets at the 
base are used sparingly in order to conserve fuel. As a 
result, the orientation and positional degrees of freedom 30 
of the base body form a 6 degree of freedom passive 
hinge. However, these degrees of freedom are ignorable 
coordinates because the kinetic energy of the manipula- 
tor does not depend on the overall orientation or posi- 
tion of the overall manipulator. As a consequence Eq. 35 
(4.7) holds for such systems. The left hand side of the 
equation is physically the spatial momentum of the 
space manipulator, while T^is the spatial force from the 
reaction jets. When the jets are completely off (i.e. 40 
Tp=0), the spatial momentum of the space manipulator 
stays constant. While the conservation of the linear 
momentum is a holonomic constraint, the conservation 
of angular momentum is a nonholonomic constant on 45 
the system. The motion and control of free-flying space 
manipulators has been extensively studied [8-11]. 

4.3 Spatial Operator Expression for M pp ~~ l 

Since M pp is the mass matrix of the passive arm A p> 50 
operator factorization and inversion techniques devel- 
oped in [4] for regular manipulators can be used to 
obtain a closed form spatial operator expression for 

First, we define the quantities 55 
P(.),D /7 (.),Gp(.),Kp(.),Tp(.),P+(.) and 1 K*,0 for the manip- 
ulator links using the following recursive algorithm. In 
the following algorithm. Hp(k) is the joint map matrix 
which maps a link vector to a joint vector and consists 
of the joint axis unit vectors of a passive joint k; P(k) is 60 
the articulated link inertia (or effective link inertia) of 
any link k; Dp(k) is the joint inertia about a passive joint 
k; Gp,(k) is a joint to link force mapping operator and 
maps a link force to a resulting passive joint accelera- 65 
tion; and, Tpfk) is a null force operator which accounts 
for the component of force resulting in no joint acceler- 
ation: 


, ( 4 -8) 
P+( 0) = 0 
for k — l ... n 

m = 4>(k,k - 1 )P+(k - 1 )4>*(k,k - 1) + M(k) 

( if k e I p 

Dp(k) = 

Gp(k) = P{k)H p *{k)D-\k) 

K^k + 1,*) = <J>(* + hk)Gp(k) 
r p (k) =1- G p (k)Hp(k) 

else 

r/k) = h 

\ end if 

p+(*> = Tptmk) 

$ (k + l,k) ss <p(k + 1 ,k)r/k) 
end loop 


16 above denotes the 6x6 identity matrix. The fore- 
going algorithm is illustrated in FIG. 3. The quantities 
defined in Eq. (4.8) are very similar to the articulated 
body quantities required for the 0(N) forward dynam- 
ics algorithm for regular manipulators [12.4]. When we 
restrict our attention to the hinges of the A p passive arm 
alone, these quantities are precisely the articulated body 
quantities for the A p manipulator. 

We can visualize the recursion in Eq. (4.8) as a walk 
from the tip to the base of the manipulator. At each 
hinge, we check whether the hinge is an active or a 
passive hinge. Depending on the status of the hinge, the 
appropriate computations are carried out and the walk 
continues on to the next hinge. This continues until the 
base body is reached. 

The operator PgS^xfiw IS defined as a block diagonal 
matrix with its k th diagonal element being P(k)e 6 x6. 
The quantities in Eq. (4.8) are also used to define the 
following spatial operators: 

L&HpPH*^ n p*p 

G p ^PH*pD p -h 6 *x*/> 

Kp=E$Gpe 6n * n P 

TpT^GpHpt 6nX6n 

E p =E$ Tp6 6nx6n ( 4 . 9 ) 

The operators D p , G p and f p are all block diagonal. 
Even though K^and E^are not block diagonal matrices, 
their only nonzero block elements are the elements 
K p (k,k— l)’s and ^,k— l)’s respectively along the first 
subdiagonal. It is easy to verify from Eq. (4.8) that P 
satisfies the equation 


M=P-E\04PE*y=P-E$PE*y ( 4 . 10 ) 

Now define the lower-triangular operator i|/eS 672 x6n as 
*=(/-£*)- 1 ( 4 . 11 ) 

Its block elements i|/(i,j)€& 6 x6 are as follows: 


n f> = 


V(ii - 1) • • . W + 1 J) for i > j 
I for i — j 

0 for i < j 


The structure of the operators $ and E^ is identical to 
that of the operators <J> and E^, except that the elements 
are now i//(i,j) rather than <f>(i,j). 
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We refer to the expression in Eq. (4.3) as the Newton- 
Euler factorization of the passive arm mass matrix, M pp . 

We now present results describing an alternative opera- 
tor factorization and inversion of M pp . Further discus- 
sion and the proofs of Lemmas 4. 1-4.3 given below can 5 
be found in references [4.5]. 

Lemma 4.1: The innovations factorization of the mass 
matrix M pp is given by 

M pp = [/ + HpbKpWjJ + HfiKpY (4.12) 10 

The factorization in Lemma 4.1 can also be regarded 
as a closed-form LDL* factorization of The closed 

form operator expression for the inverse of the factor 
[I+Hp<f>Kp] is described in Lemma 4.2 below. 15 

Lemma 4.2: [/ + HfiKjr 1 = [/ - H^K p ] ■ 

Combining Lemma 4.1 and Lemma 4.2 leads to the 2Q 
following closed form operator expression for the in- 
verse of the mass vertrix M pp . 

Lemma 4.3: M~ l - [/ - H^K p ]*D p l [I - H^K p ] ■ 

25 

The factorization in Lemma 4.3 can be regarded as a 
closed-form LDL* factorization of M pp ~ l . Using the 
operator expression for the inverse of M pp in Le mm a 4.2 
together with Eq. (4.5), the following lemma describes 
new closed form operator expressions for the S,y matri- 30 
ces. 

Lemma 4.4: 


(4.13a) 
(4.13b) 35 

Saa = - PClF\H a * ‘ (4.13c) 

= - PSl)P + P$*]H a * 

where 

fl t rHp'D-'H^ (4.14) 40 

■ 

The expressions for the S $ matrices in Lemma 4.4 
require only the inverse of the block-diagonal matrix 
— an inverse that is relatively easy to obtain. 45 

5. Recursive 0(N) Dynamics Algorithm 

We now derive a recursive 0(N) algorithm for the 
dynamics of under-actuated manipulators. One of the 
primary computations for control is the computation of 50 
the actuator generalized forces — To — needed to obtain 
a desired acceleration fi a at the active hinges, and the 
induced accelerations fi p at the passive hinjges. We first 
use Lemma 4.4 to obtain expressions for fi p , the active 
hinge forces vector T c , and the link spatial accelerations 55 
vector /3. As described in the lemma below, we intro- 
duce the new quantities Z,e p and v p to simplify these 
expressions. 



Lemma 5.1: We have 


60 


a = $*[H p *v p + H a * Pa + d \ (5.1a) 

P p =[I~ H^K p ]*v p - K p *r[H a *ka + a) = v p - K p *a (5.1b) 


Ta = Ha {: + P^*v p - $*{H a *Po - *)]} = 


(5-lc) 65 


H a P[a — H a *p a — a] + z 


14 

-continued 

Z l = MKpTp + b + P(Ha*i 13a + a)] 
= T p - Hpz 
V P = D F' € P 


The ability to convert spatial operator expression into 
fast recursive algorithms by inspection is one of the 
major advantages of the spatial operator approach. This 
is a direct consequence of the special structure of the 
operators such as <J> and iff and is discussed in more detail 
in references [4.5]. We use this feature to convert the 
closed form operator expressions for the vectors fi p T a 
in Lemma 5.1 into a recursive 0(N) computational 
algorithm. This algorithm requires a recursive tip-to- 
base sweep followed by a base-to-tip sweep as described 
below. In the following algorithm, f(k) is a link interac- 
tion force between adjoining links; z(k) is a residual link 
force attributable to coupling of passive and active joint 
forces of preceding links; c p (k) is the resulting joint 
force on a passive joint due to the passive joint force 
and the residual link force; and, v^(k) is the resulting 
joint acceleration due to the resulting joint force; 


( m = o 

I for k = 1 ... n 


(5.2a) 


/ if e l a 

m = <Kfc* - !)*+(* - 1) + m + 

miV(k)Mk) + a(k)] 

2 +(*) = m 

else 

m = <Kfc* - l)z+(* - 1) + m + Kk)a(k) 

«#) = Tpfk) - H(k)m 
z+(k) = zik) + Gpifytptk) 

v I m = D p h I m 

\ end if 
^ end loop 


, (5-2b) 

a+(n + 1) - 0 
for & = «... 1 

a+(k) « <j>*(k + l,k)a(k + 1) 

( if ke I a 

A® - Kk)a+{k) + z(k) 

TJk) = H{Wk) 
else 

W = M k ) ~ fyk)a+(k) 

^ end if 

a(k) = a+(*) + H\k)p(k) + a(k) 

^ end loop 

An overview of the foregoing algorithm is illustrated 
in FIG. 4. The algorithm is illustrated in detail in FIG. 
5a and FIG. 5b. 

Once again we can visualize the recursions in Eq. 
(5.2) as a walk across the links of the manipulator. The 
walk starts from the tip to the base in Eq. (5.2a). At each 
hinge, its active/passive status is checked. If the hinge is 


where 
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active, its acceleration is known, and it is used to update 
the residual force z(.). On the other hand, if the hinge is 
passive, its generalized force is known, and it is used 
instead to update the residual force. This part of the 
walk continues until the base is reached. Now a new 5 
walk from the base towards the tip is begun. This time 
as each new hinge is encountered, if it is passive, its 
unknown acceleration is computed, while if the hinge is 
active, its unknown generalized force is computed. This 
continues until the tip is reached and all the hinges have 10 
been processed. In summary, this dynamics algorithm 
requires the following 3 steps illustrated in FIG. 6: 

1. The recursive computation of all the link velocities 
V(k), and the Coriolis terms a(k) and b(k) using a base- 
to-tip recursion sweep as in the standard Newton-Euler 
inverse dynamics algorithm in Eq. (3.1). 

2. Recursive computation of the articulated body 
quantities using the tip-to-base recursive sweep de- 
scribed in Eq. (4.8). 

3. The inward tip-to-base recursive sweep in Eq. 20 
(5.2a) to compute the residual forces z(k). This is fol- 
lowed by the base-to-tip recursive sweep in Eq. (5.2b) 

to compute the fip, T a and a. 

In accordance with FIG. 6, in one embodiment of the 
invention the foregoing three steps comprise a cycle 25 
which is repeated in successive time steps. This is useful, 
for example for performing robot motion planning to 
generate a sequence of active joint forces (and corre- 
sponding servo commands) to achieve a desired robot 
motion. At each time step, the active joint accelerations 30 
have been predetermined in accordance with the de- 
sired robot motion (e.g., a user-desired end-effector 
velocity profile), and the active joint accelerations for 
the current time step are retrieved in Step 1 above. The 
Coriolis accelerations a(k) and centrifugal forces b(k) of 35 
the active links are computed by integrating the active 
joint accelerations. However, the passive joint accelera- 
tions for the current time step are determined in Step 2 
and are therefore unknown in Step 1. Accordingly, the 
Coriolis accelerations a(k) and centrifugal forces b(k) of 40 
the passive links must be computed by integrating the 
passive joint accelerations computed during the previ- 
ous time step. 

Note that the recursions in Step (2) can be combined 
and carried out in conjunction with the tip-to-base 45 
sweep in Step (3). Since each recursive step for a link in 
the above algorithm has a fixed computational cost, the 
overall computational cost of the algorithm is linear in 
both N a and Np, i.e. linear in N. The algorithm is there- 
fore an 0(N) algorithm. Note however that the costs 50 
associated with passive degrees of freedom is larger 
than that for active hinge. Note that the overhead asso- 
ciated with transitions between passive and active status 
of the hinge is very small for this algorithm. When such 
a transition occurs during run-time, the only change 55 
required is to update the sets I p or l a . 

An interesting feature of this dynamics algorithm is 
that its structure is a hybrid of known inverse and for- 
ward dynamics algorithms for regular manipulators. 
When all the hinges are passive, l a is empty and the 60 
steps in the above algorithm reduce to the well known 
0(N) articulated body forward dynamics algorithm 
[12.4]. In this case, P(k) is the articulated body inertia of 
all the links outboard of the k th link. In the other ex- 
treme case, when all the hinges are active hinges, I p is 
empty, and the steps in the algorithm reduce to the 
composite rigid body inertias based 0(N) inverse dy- 
namics algorithm [5]. In this case, P(k) is the composite 
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rigid body inertia of all the links that are outboard of the 
k th link. 

6. Kinematical Quantities 

In this section we study the kinematical properties of 
under-actuated manipulators. The term kinematical is 
used rather loosely to parallel its use for regular manip- 
ulators. As we shall see, “kinematical” quantities such 
as the Jacobians etc. depend not only upon the kinemati- 
cal quantities such as link lengths etc., but also upon 
dynamical quantities such .as link inertias and masses. 
We begin by looking at the permissible virtual displace- 
ments for the under-actuated manipulator and in the 
process define a projection operator which plays a fun- 
15 damental role in the defining the structure of kinemati- 
cal and dynamical quantities for the manipulator. For 
the remainder of this section, we assume that the gener- 
alized force vector for the passive hinges T p is zero. 

6.1 The Projection Operators T and T 
The operators T and T are defined as follows: 

and 

The following lemma shows that T and T are indeed 
projection operators. 

Lemma 6.1: The operators T and T are projection 
operators, i.e., 

T 2 =T. and T 2 =7 (6.1) 

Moreover, rank[T]=N^, and rank[T]=N a . Also, 

r<f>*=7ty*==xfj*~{lP ( 6 . 2 ) 

T<t>*H p *=T\!f*H p *=0 ( 6 . 3 ) 

The notion of virtual displacements plays a funda- 
mental role in understanding the behavior of dynamical 
systems. Virtual displacements characterize the possible 
“permissible” motions of constrained bodies. The vir- 
tual displacement matrix defines the mapping between a 
displacement of the generalized velocities and the dis- 
placement of the component bodies. From Eq. (3.2) the 
spatial acceleration a of the links in a regular manipula- 
tor is given by 

a =4>*H m &+ velocity dependent terms 

Hence the virtual displacement matrix V for a regular 
manipulator is given by 

v==4>*H* ( 6 . 4 ) 

Similarly, the virtual displacement matrix V c for the 
active arm A a is given by 

V a ~§*H a * ( 6 . 5 ) 

Lemma 6.2: The virtual displacement matrix Vjyfor 
the under-actuated manipulator is given by 

Vu = = TV a = TV (6.6) 

The virtual displacement matrix for an under- 
65 actuated arm is related to the virtual displacement ma- 
trices for both the regular and the active arm A a via the 
projection operator T. It follows directly from Eq. (6.6) 
that if 8 is a permissible virtual displacement for the 
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regular manipulator (or for the active arm A fl ), then T8 
is a valid virtual displacement for the under-actuated 
manipulator. Since T is of rank N c , the role of T is to 
project the virtual displacement of the regular arm from 
a N dimensional tangent space onto the smaller N G 5 
dimensional tangent space for under-actuated manipula- 
tors. 

From the expression for T a in Eq. (4.4) it is apparent 
that the mass matrix for the under-actuated manipulator 
Mu is given by S aa . A new expression for the mass 10 
matrix based upon the projection operator T is de- 
scribed in the following lemma. 

Lemma 6.3: 

Mu =Saa = Ha<f>(T*MT)4>*Ha* = (6.7) 15 

Note that the mass matrix of the regular manipulator 
is given by while that of the active arm A^ 

is given by Thus the mass matrix Mu of 

the under-actuated manipulator is related to the mass 20 
matrices of the corresponding regular and active manip- 
ulators in a simple manner by the projection operator T. 
Indeed, in this case, replacing M by T*MT results in a 
regular/active manipulator with the same mass matrix 
as that of the under-actuated manipulator. 25 

6.2 Jacobians for Under-Actuated Manipulators 

Motion planning and control algorithms for regular 
manipulators make use of the end-effector Jacobian of 
the manipulator. Moreover, many of the key Jacobian 30 
related computations can be carried out by means of 
efficient recursive algorithms. But for the increase com- 
plexity, these statements are also true for under- 
actuated manipulators. 

6.2.1 The Generalized Jacobian J g 35 

The generalized Jacobian Jg defines the relationship 
between the motion of the active hinges and the motion 
of the end-effector frame as described below: 

* 40 

a(0) =JG & a + velocity dependent terms 

The generalized Jacobian is important for describing 
the local behavior of the end effector. The expression 
for Jo is given in the following lemma. 

Lemma 6.4: The generalized Jacobian Jg for an un- 45 
deractuated manipulator is given by 

Jg=B*T 4> *Ha *—B*T^> *H*=B*\)\t *- &F\H a * (6.8) 

where fl is defined in Equation 4.14 above. 50 

Proof: This follows from Lemma 6.2 and the fact that 
a(0)=B*a. ■ 

We see from Eq. (6.8) that for under-actuated manip- 
ulators, the kinematical as well as the inertial properties 
of the links enter into the structure of the Jacobian via 55 
the projection operator T. In contrast, for regular ma- 
nipulators, the end-effector Jacobian is purely a func- 
tion of the kinematical properties of the manipulator. 
Comparing with Eq. (3.5), we see that the deviation of 
Jg from the Jacobian of the regular manipulator, J, is 60 
given by the projection operator T. For space manipula- 
tors, singularity analysis of Jg is used to study the desir- 
able and undesirable regions of the workspace [11] as 
well as for control using methods such as Resolved Rate 
Control [9]. Similar techniques can also be applied to 65 
the class of under-actuated manipulators as well. 

The computation of Jccan be carried out recursively. 
For these computations, all the hinge velocities are set 
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to zero. This implies that the nonlinear velocity depen- 
dent terms a(k) and b(k) are zero for all links. Next, all 
the articulated body quantities are computed using the 
tip-to-base recursion in Eq. (4.8). The following proce- 
dure illustrated in FIG. 7 then results in the computa- 
tion of the k th column of Jg: 

1. Set /3 c (k)= 1, and all the other active hinge acceler- 
ations to 0. 

2. Use the tip-to-base and base-to-tip recursions in Eq. 
(5.2) to compute the spatial acceleration a(l) of the 
end-effector link. 

3. The k th column of Jg is <j>*(l,0)a(l). 

Repeating this procedure for each of the n a columns 
yields the complete generalized Jacobian matrix Jg- 
Storage of some of the intermediate quantities can be 
used to reduce the computational cost of the above 
procedure. For a given vector x, setting /3 a =x and 
carrying out only a single evaluation Step (2) above 
results in the computation of the matrix- vector product 
JgX. While the recursive structure of the algorithm is in 
line with the recursive structure of similar computations 
for regular manipulators, the Step (2) above requires a 
tip-to-base recursion in addition to the single base-to-tip 
recursion needed for regular manipulators. 

6.2.2 Computations of the Actuator Forces 
Corresponding to a Desired End-Effector Trajectory 

The generalized Jacobian can be used to compute the 
active hinge generalized forces T a (t) required to 
achieve a desired end-effector time trajectory as defined 
by its spatial acceleration ao(t). We assume that the state 
of the manipulator is known at the beginning, i.e., the 
configuration 0(to) and hinge velocities /3(to) are known 
at time t~ to and that an integration time step At is being 
used. A brief sketch of the computational steps at time 
t is described below, in accordance with the illustrations 
of FIG. Sa: 

1. Compute Jc(t). 

2. Compute /3 a (t) via 

fidf) —J g~ 1 (^[“oCO — velocity dependent terms] 

3. Compute T fl (t) and £ p (t) using the algorithm in 
Section 5. 

4. Integrate the hinge accelerations and velocities to 
obtain the hinge velocities /3(t+At) and positions 
0(t-f At) at time t+ At Go back to Step 1 and repeat the 
steps for time (t-j-At). 

The velocity dependent terms in Step 2 are as fol- 
lows: 

B*^*{H p *D p - '[T p -H p y{K p Tp+b+Pa)]+a} 

+c(0). 

This iterative procedure results in a time profile for 
the actuator forces T a (t) required to achieve the desired 
end-effect trajectory. It also results in the trajectory of 
the passive hinges for the whole time interval as well. 
For simplicity, we assumed above that Jg is square and 
non-singular. This procedure can be modified (as in the 
usual ways for regular manipulators) when the Jacobian 
is singular, or there are redundant active hinge degrees 
of freedom available, or only a subset of end-effector 
variables are. specified (such as for pointing applica- 
tions). 
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6.2.3 The Disturbance Jacobian J d 

In applications where the number of active degrees of 
freedom is larger than that needed to meet the primary 
task (such as end-effector motion) for the manipulator, 5 
the extra active_degrees of freedom can be used to meet 
other secondary objectives. One such important objec- 
tive is the optimization of the motion of the passive 
hinges for the purpose of minimizing disturbance or for 
collision avoidance etc. The disturbance Jacobian Jijis io 
the key for studying the inertial coupling between the 
active and the passive hinges. This Jacobian describes 
the “disturbance” motion induced into the passive 
hinges due to the motion of the active hinges as follows 

... 15 

(Sp—Jofia + velocity dependent terms 


In [8], J/jhas been used to study the motions of a space 
robot which minimizes the the disturbance that the 
manipulator imparts to the base body. 

Lemma 6.5: The operator expression for the distur- 
bance Jacobian Id of an under-actuated manipulator is 
given by: 


Jd= -S ap *= — [I—HptyKp] *D p — 
PH a *—K p *ty *H a * 


( 6 . 9 ) 25 


Proof: This follows from Eq. (4.13b). ■ 

The computation of Ji> can be carried out simulta- 
neously with the computation of Jg via the algorithm 
described earlier in Section 6.2.1. Thus the k th column 30 
of J/>is simply the vector $ p computed during the steps 
leading to the computation of the k th column of Jg- 
FIG. 8 b illustrates how the disturbance Jacobian 
operator Jz>is used to control a redundant robot manip- 
ulator (a manipulator having at least seven degrees of 35 
freedom). As shown in FIG. 8 b, the disturbance Jaco- 
bian operator is computed in accordance with the pro- 
cess discussed previously with reference to FIG. 7. 
Then, the unknown passive joint accelerations are com- 
puted by multiplying the active joint accelerations (ob- 40 
tained, for example, via the generalized Jacobian opera- 
tor as shown in FIG. &z). At this point, all of the joint 
accelerations are known, permitting computation of any 
changes in velocity or position. These changes are then 
combined with a set of predetermined redundancy re- 45 
solving motion constraints by a redundancy resolution 
control process, which ultimately solves the robot con- 
trol equations an outputs robot servo commands. Such 
redundancy resolution control processes are known in 
the art and one is disclosed, for example, in U.S. patent 50 
application Ser. No. 07/849,629 filed Mar. 1 1, 1992 by 
Homayoun Seraji, Mark K. Long and Thomas S. Lee, 
entitled “Configuration Control of Seven Degree of 
Freedom Arms” and assigned to the present assignee. 

FIG. 9 illustrates one exemplary system embodying 55 
the present invention and employing the processes de- 
scribed above. The system includes a robot control 
computer 100 and a robot 102 having joint servos 104 
and joint sensors 106. The current link postions 108 and 
link velocities 110 are determined from either the joint 60 
sensors 106 or from previously computed accelerations 
112 via Newton-Euler algorithm 114 in a processor 116. 
Then, a processor 118 computes the articulated body 
quantities from the link positions 108. A processor 120 
then computes the generalized Jacobian operator 122 65 
from the articulated body quantities using the modified 
recursive dynamics algorithm of FIG. 7. User-specified 
end-effector accelerations 124 (for example) are then 
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multiplied (126) by the generalized Jacobian operator 
122 to produce the active joint accelerations 128. Mean- 
while, a processor 130 computes the coriolis link accel- 
erations and centrifugal link forces from the link veloci- 
ties 110. Finally, a processor 132 computes the un- 
known active joint forces and passive joint accelera- 
tions from the active joint accelerations 128, the articu- 
lated body quantities computed by the processor 118 
and the coriolis and centrifugal terms computed by the 
processor 130 via the recursive dynamics algorithm of 
FIG. 4. A processor 134 generates joint servo com- 
mands from the active joint forces and transmits these 
commands to the joint servos 104. Alternatively, the 
processor 134 stores these commands in memory if a 
time profile of servo commands is to be computed in a 
motion planning mode prior to execution by the robot 
102 . 

While the invention has been described in detail by 
specific reference to preferred embodiments thereof, it 
is understood that variations and modifications thereof 
may be made without departing from the true spirit and 
scope of the invention. 

What is claimed is: 

1. A method of controlling a robot manipulator 
which is under-actuated having plural links connected 
at respective joints therebetween whereby each link is 
associated with a respective joint, some of said joints 
being active and having their movements controlled by 
respective joint servos, remaining ones of said joints 
being passive and not having their movements con- 
trolled by respective joint servos whereby said passive 
joints are generally freely moveable, an innermost one 
of said links comprising a relatively stationary base and 
an outermost one of said links comprising a moveable 
tip link, said method comprising the steps of: 

obtaining current positions of said links, specified 
active joint accelerations of said active joints and 
passive joint forces of said passive joints; 

computing articulated body^ quantities for each of said 
joints from said current positions of said links; 

- computing from said articulated body quantities and 
from said active joint accelerations and said passive 
joint forces, active joint forces of said active joints 
by performing a recursive dynamics algorithm; and 

transmitting servo commands corresponding to said 
active joint forces to respective ones of said joints 
servos so as to correct motion of said robot manip- 
ulator. 

2. The method of claim 1 wherein said recursive 
dynamics algorithm comprises: 

an inward recursion comprising the following steps 
carried out for each link beginning with said outer- 
most link: 

computing a residual link force from a residual link 
force of a previous link, from a corresponding one 
of said active joint accelerations if the correspond- 
ing joint is active and from a corresponding one of 
said passive joint forces if the corresponding joint 
is passive: 

an outward recursion comprising the following steps 
carried out for each link beginning with said inner- 
most link: 

computing a link acceleration from a link accelera- 
tion of a previous link; 

computing from said link acceleration an active 
joint force if the corresponding joint is an active 
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joint, and computing a passive joint acceleration 
if the corresponding joint is a passive joint; and 
revising said link acceleration based upon the joint 
acceleration of the corresponding joint. 

3. The method of claim 1 wherein said articulated 
body quantities_comprise an effective link inertia, a joint 
inertia, a joint to link force operator and a null force 
operator. 

4. The method of claim 3 wherein the computing 
articulated body quantities comprises an inward recur- 
sion comprising the following steps carried out for each 
link beginning at said outermost link: 

computing said effective link inertia from the effec- 
tive link inertia of a previous link; 
if the corresponding joint is passive: 
computing said joint inertia from said effective link 
inertia; 

computing said joint to link force operator from said 
effective link inertia and from said joint inertia; and 
computing said null force operator from said joint to 
link force operator; 

if the corresponding joint is active, setting said null 
force operator equal to an identity operator; and 
revising said effective link inertia by transforming 
said effective link inertia by said null force opera- 
tor. 

5. The method of claim 1 wherein the obtaining speci- 
fied active joint accelerations comprises: 

obtaining a desired acceleration of a selected one of 
said links: 

computing a generalized Jacobian operator relating 
said desired acceleration of said selected one link to 
the active joint accelerations of said active joints: 
multiplying said desired acceleration of said selected 
one link by said generalized Jacobian operator to 
produce said active joint accelerations of said ac- 
tive joints. 

6. The method of claim 5 wherein said one selected 
link is said outermost link. 

7. The method of claim 5 wherein said generalized 
Jacobian operator comprises a matrix having plural 
columns and wherein the computing of said generalized 
Jacobian operator comprises: 

setting the active joint acceleration of a successive 
one of said active joints to unity and the active joint 
accelerations of the remaining active joints to zero; 
for each setting of the active joint acceleration of a 
successive one of said active joints to unity, per- 
forming the following recursive dynamics algo- 
rithm: 

an inward recursion comprising the following step 
carried out for each link beginning with said 
outermost link: 

computing a residual link force from a residual 
link force of a previous link, from a corre- 
sponding one of said active joint accelerations 
if the corresponding joint is active and from a 
corresponding one of said passive joint forces 
if the corresponding joint is passive; 
an outward recursion comprising the following 
steps carried out for each link beginning with 
said innermost link: 

computing a link acceleration from a link accel- 
eration of a previous link; 
computing from said link acceleration an active 
joint force if the corresponding joint is an 
active joint, and computing a passive joint 
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acceleration if the corresponding joint is a 
passive joint; and 

revising said link acceleration based upon the 
joint acceleration of the corresponding joint; 

5 storing the link acceleration of the outmost link as a 
successive one of the columns of said matrix of said 
generalized Jacobian operator. 

8. The method of claim 1 wherein said passive joint 
forces correspond to friction forces of said passive 

10 joints. 

9. The method of claim 1 wherein said robot manipu- 
lator comprises joint sensors in each of said joints, and 
wherein said obtaining of said current positions of said 
links comprises reading said joint sensors. 

15 10. The method of claim 1 wherein said obtaining, 

said computing articulated body quantities and said 
computing said active joint forces comprise a single 
cycle, said method further comprising repeating said 
single cycle periodically in successive time steps 
20 whereby to store a time profile of said active joint 
forces, whereby said transmitting servo commands is 
performed upon completion of a plurality of said cycles 
corresponding to a desired movement of said robot 
manipulator. 

25 11 . A method of controlling a robot manipulator 

which is redundant and under-actuated having at least 
seven degrees of freedom, whereby equations of motion 
of said robot manipulator have a redundancy, said robot 
manipulator comprising plural links connected at re- 
30 spective movable joints therebetween whereby each 
link is associated with a respective joint, some of said 
joints being active and having their movements con- 
trolled by respective joint servos, remaining ones of said 
joints being passive and not having their movements 
35 controlled by respective joint servos whereby said pas- 
sive joints are generally freely moveable, an innermost 
one of said links comprising a relatively stationary base 
and an outermost one of said links comprising a move- 
able tip link, said method comprising the steps of: 

40 obtaining current positions of said links, specified 
active joint accelerations of said active joints and 
passive joint forces of said passive joints; 

computing articulated body quantities for each of said 
joints from said current positions of said links; 

45 computing from said articulated body quantities and 
from said active joint accelerations and said passive 
joint forces, a disturbance Jacobian operator relat- 
ing said active joint accelerations of said active 
joints to passive joint accelerations of said passive 
50 joints; 

computing said passive joint accelerations by combin- 
ing said disturbance Jacobian operator and said 
active joint accelerations; 

establishing a set of constraints resolving said redun- 
55 dancy, and computing active joint forces from said 

set of constraints and from said active and passive 
joint accelerations and transmitting joint servo 
commands corresponding to said active joint 
forces to said active joints so as to correct motion 
60 of said robot manipulator. 

12. The method of claim 11 wherein said disturbance 
Jacobian operator comprises a matrix having plural 
columns, and wherein the computing of said distur- 
bance Jacobian operator comprises: 

65 setting an active joint velocity of a successive one of 
said active joints to unity while setting the active 
joint velocities of the remaining active joints to 
zero; 
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for each setting of the active joint acceleration of a 
successive one of said active joints to unity, per- 
forming the following recursive dynamics algo- 
rithm: 

an inward recursion comprising the following step 
carried out for each link beginning with said 
outermost link: 

computing a residual link force from a residual 
link force of a previous link, from a corre- 
sponding one of said active joint accelerations 
if the corresponding joint is active and from a 
corresponding one of said passive joint forces 
if the corresponding joint is passive; 

an outward recursion comprising the following steps 
carried out for each link beginning with said inner- 
most link: 

computing a link acceleration from a link accel- 
eration of a previous link; 
computing from said link acceleration an active 
joint force if the corresponding joint is an 
active joint, and computing a passive joint 
acceleration if the corresponding joint is a 
passive joint; and 

revising said link acceleration based upon the 
joint acceleration of the corresponding joint; 

storing said passive joint accelerations as a successive 
one of the columns of said matrix of said distur- 
bance Jacobian operator. 

13. Apparatus for controlling an under-actuated 
robot manipulator having plural links connected at 
respective movable joints therebetween whereby each 
link is associated with a respective joint, some of said 
joints being active and having their movements con- 
trolled by respective joint servos, remaining ones of said 
joints being passive and not having their movements 
controlled by respective joint servos whereby said pas- 
sive joints are generally freely moveable, an innermost 
one of said links comprising a relatively stationary base 
and an outermost one of said links comprising a move- 
able tip link, said apparatus comprising: 

means for obtaining current positions of said links, 
specified active joint accelerations of said active 
joints and passive joint forces of said passive joints; 

means for computing articulated body quantities for 
each of said joints from said current positions of 
said links; 

means for computing from said articulated body 
quantities and from said active joint accelerations 
and said passive joint forces, active joint forces of 
said active joints by performing a recursive dynam- 
ics algorithm; and 

means for transmitting servo commands correspond- 
ing to said active joint forces to respective ones of 
said joint servos so as to correct motion of said 
robot manipulation. 

14. The apparatus of claim 13 wherein said means for 
performing a recursive dynamics algorithm comprises: 

means for computing, for each link successively be- 
ginning with said outermost link, a residual link 
force from a residual link force of a previous link, 
from a corresponding one of said active joint accel- 
erations if the corresponding joint is active and 
from a corresponding one of said passive joint 
forces if the corresponding joint is passive; 

means for computing, for each link successively be- 
ginning with said innermost link, a link accelera- 
tion from a link acceleration of a previous link; 
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means for computing from said link acceleration an 
active joint force if the corresponding joint is an 
active joint, and computing a passive joint acceler- 
ation if the corresponding joint is a passive joint, 
for each link successively beginning with said in- 
nermost link; and 

means for revising, for each link successively begin- 
ning with said innermost link, said link acceleration 
based upon a joint acceleration of the correspond- 
ing joint. 

15. The apparatus of claim 13 wherein said articulated 
body quantities comprise an effective link inertia, a joint 
inertia, a joint to link force operator and a null force 
operator. 

16. The apparatus of claim 15 wherein the means for 
computing articulated body quantities comprises an 
inward recursion processor operative for each link be- 
ginning at said outermost link, comprising: 

means for computing said effective link inertia from 
the effective link inertia of a previous link; 

means operative if the corresponding joint is passive 
for computing: 

said joint inertia from said effective link inertia; 

said joint to link force operator from said effective 
link inertia and from said joint inertia; and 

said null force operator from said joint to link force 
operator; 

means operative if the corresponding joint is active 
for setting said null force operator equal to an iden- 
tity operator; and 

means for revising said effective link inertia by trans- 
forming said effective link inertia by said null force 
operator. 

17. The apparatus of claim 13 wherein the means for 
obtaining specified active joint acceleration comprises: 

means for obtaining a desired acceleration of a se- 
lected one of said links; 

means for computing a generalized Jacobian operator 
relating said desired acceleration of said selected 
one link to the active joint accelerations of said 
active joints; 

means for multiplying said desired acceleration of 
said selected one link by said generalized Jacobian 
operator to produce said active joint accelerations 
of said active joints. 

18. The apparatus of claim 17 wherein said one se- 
lected link is said outermost link. 

19. The apparatus of claim 17 wherein said general- 
ized Jacobian operator comprises a matrix having plural 
columns and wherein the means for computing said 
generalized Jacobian operator comprises: 

means for setting the active joint acceleration of a 
successive one of said active joints to unity and the 
active joint accelerations of the remaining active 
joints to zero; 

means operative for each setting of the active joint 
acceleration of a successive one of said active joints 
to unity, for performing a recursive dynamics algo- 
rithm, comprising: 

means for computing, for each link beginning with 
said outermost link, a residual link force from a 
residual link force of a previous link, from a 
corresponding one of said active joint accelera- 
tions if the corresponding joint is active and from 
a corresponding one of said passive joint forces if 
the corresponding joint is passive; 
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means for computing, for each link beginning with 
said innermost link, a link acceleration from a link 
acceleration of a previous link; 

means for computing from said link acceleration an 
active joint force if the corresponding joint is an 5 
active joint, and computing a passive joint acceler- 
ation if the corresponding joint is a passive joint, 
for each link beginning with said innermost link; 

means for revising, for each link beginning with said 
innermost link, said link acceleration based upon 1 ° 
the joint acceleration of the corresponding joint; 
and 

means for storing the link acceleration of the outer- 
most link as a successive one of the columns of said 
matrix of said generalized Jacobian operator. 15 

20. The apparatus of claim 13 wherein said passive 
joint forces correspond to friction forces of said passive 
joints. 

21. The apparatus of claim 13 wherein said robot 
manipulator comprises joint sensors in each of said 20 
joints, and wherein said means for obtaining said posi- 
tions of said links comprises means for reading said joint 
sensors. 

22. Apparatus for controlling a robot manipulator ^ 

which is redundant and under-actuated having at least 2 
seven degrees of freedom, whereby equations of motion 
of said under-actuated robot manipulator have a redun- 
dancy, said robot manipulator comprising plural links 
connected at respective movable joints therebetween 3Q 
whereby each link is associated with a respective joint, 
some of said joints being active and having their move- 
ments controlled by respective joint servos, remaining 
ones of said joints being passive and not having their 
movements controlled by respective joint servos 35 
whereby said passive joints are generally freely move- 
able, an innermost one of said links comprising a rela- 
tively stationary base and an outermost one of said links 
comprising a moveable tip link, said apparatus compris- 
ing: 40 

means for obtaining current positions of said links, 
specified active joint accelerations of said active 
joints and passive joint forces of said passive joints; 

means for computing articulated body quantities for 
each of said joints from said current positions of 45 
said links; 

means for computing from said articulated body 
quantities and from said active joint accelerations 
and said passive joint forces, a disturbance Jaco- 
bian operator relating said active joint accelera- 50 
tions of said active joints to passive joint accelera- 
tions of said passive joints; 

means for computing said passive joint accelerations 
by combining said disturbance Jacobian operator 
and said active joint accelerations; 55 

means for establishing a set of constraints resolving 
said redundancy, and computing active joint forces 
from said set of constraints and from said active 
and passive joint accelerations and transmitting 
joint servo commands corresponding to said active 60 
joint forces to said active joints so as to correct 
motion of said robot manipulator. 

23. The apparatus of claim 22 wherein said distur- 
bance Jacobian operator comprises a matrix having 
plural columns, and wherein the means for computing 65 
said disturbance Jacobian operator comprises: 

means for setting an active joint velocity of a succes- 
sive one of said active joints to unity while setting 
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the active joint velocities of the remaining active 
joints to zero; 

means operative for each setting of the active joint 
acceleration of a successive one of said active joints 
to unity, for performing a recursive dynamics algo- 
rithm, comprising: 

means for computing, for each link beginning with 
said outermost link, a residual link force from a 
residual link force of a previous link, from a 
corresponding one of said active joint accelera- 
tions if the corresponding joint is active and from 
a corresponding one of said passive joint forces if 
the corresponding joint is passive; 
means for computing, for each link beginning with 
said innermost link, a link acceleration from a 
link acceleration of a previous link; 
means for computing from said link acceleration an 
active joint force if the corresponding joint is an 
active joint, and a passive joint acceleration if the 
corresponding joint is a passive joint, for each 
link beginning with said innermost link; 
means for revising, for each link beginning with 
said innermost link, said link acceleration based 
upon the joint acceleration of the corresponding 
joint; and 

means for storing said passive joint accelerations as a 
successive one of the columns of said matrix of said 
disturbance Jacobian operator. 

24. A method of controlling an under-actuated robot 
manipulator comprising active joints associated with 
active links and passive joints associated with passive 
links, including an outermost one of said links and an 
innermost one of said links, said method comprising the 
steps of: 

obtaining predetermined active joint accelerations of 
the active joints and passive joint friction forces of 
the passive joints; 

computing articulated body quantities for each of the 
passive and active joints from the current positions 
of the passive and active links; 

computing from the articulated body quantities and 
from the active joint accelerations and the passive 
joint friction forces, active joint forces of the active 
joints by performing a recursive dynamics algo- 
rithm; and 

producing and transmitting servo commands corre- 
sponding to the active joint forces to respective 
ones of the joint servos so as to correction motion 
of said robot manipulator. 

25. The method of claim 24 wherein the computing of 
the active joint forces comprises: 

computing for each link beginning with the outer- 
most link the residual link force of each link; 

computing for each link beginning with the innermost 
link the active joint force from the residual link 
force if the corresponding joint is active. 

26. The method of claim 25 wherein the computing of 
the residual link force comprises computing said resid- 
ual link force from the active joint acceleration if the 
corresponding joint is active or from the passive joint 
friction force if the corresponding joint is passive. 

27. The method of claim 25 wherein the computing of 
the active joint force is replaced by computing a passive 
joint acceleration acceleration from the residual link 
force if the corresponding joint is passive, and wherein 
the computing of the active joint force of a subsequent 
link is affected by said passive joint acceleration. 

* * * * * 



